#include "ddmcmc_edge.h"
#include <opencv2/highgui/highgui.hpp>
#include <cmath>

DDMCMC_EdgePrcs::DDMCMC_EdgePrcs(Mat mImage) {
	mRawImage = mImage;
	bColor = true;		// Assume all images are color images
	rows = mImage.rows;
	cols = mImage.cols;
	dSigma = 0.5;

	nMaxContourLength = 100000;
	nMinContourLength = 2;

	mLocalMax.create(rows, cols, CV_8U);
	mLocalMaxCJ.create(rows, cols, CV_8U);
	mLocalMaxFlag.create(rows, cols, CV_8U);
	mEdgeAngle.create(rows, cols, CV_32F);
	mEdgeStrength.create(rows, cols, CV_32F);

	// Initialize the search directions
	char *c = new char[512];
	strcpy(c, 
	   "1,2,3,4,5,6,7,8,0,\
		7,8,1,2,3,0,0,0,0,\
		8,1,2,3,4,0,0,0,0,\
		1,2,3,4,5,0,0,0,0,\
		2,3,4,5,6,0,0,0,0,\
		3,4,5,6,7,0,0,0,0,\
		4,5,6,7,8,0,0,0,0,\
		5,6,7,8,1,0,0,0,0,\
		6,7,8,1,2,0,0,0,0,");
	char* temp = c;
	for (int i = 0; i < 9; i++) {
		for (int j = 0; j < 9; j++) {
			sscanf(temp, "%d", &(mSearchDirection[i][j]));
			temp += 2;
		}
		temp += 2;
	}
	delete[] c;

	// Initialize the neighbor of a points
	aPointNeighbor[0].x = 1;
	aPointNeighbor[0].y = 0;
	aPointNeighbor[1].x = 1;
	aPointNeighbor[1].y = 1;
	aPointNeighbor[2].x = 0;
	aPointNeighbor[2].y = 1;
	aPointNeighbor[3].x = -1;
	aPointNeighbor[3].y = 1;
	aPointNeighbor[4].x = -1;
	aPointNeighbor[4].y = 0;
	aPointNeighbor[5].x = -1;
	aPointNeighbor[5].y = -1;
	aPointNeighbor[6].x = 0;
	aPointNeighbor[6].y = -1;
	aPointNeighbor[7].x = 1;
	aPointNeighbor[7].y = -1;
	
}

DDMCMC_EdgePrcs::~DDMCMC_EdgePrcs() {

}

/* Skeletonize edges						*/
/* junction_filter is of form 012 			*/
/*						      3 4 			*/
/*							  576 			*/
int DDMCMC_EdgePrcs::skeletonize(Mat &mEdges) {
	int jmap[256] = {
		1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 0, 1, 1, 0, 0,
		1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0,
		1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 1, 0, 0,
		1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 1, 0, 1,
		1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
		1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
		1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 0, 1, 0, 1,
		0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1,
		1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0,
		1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 1,
		0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
		0, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 1, 0, 1, 1, 1,
		1, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 1,
		1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1,
		0, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 1, 0, 1, 1, 1,
		0, 0, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1 };

	bool complete = false;
	int minc = 1, maxc = cols - 2, minr = 1, maxr = rows - 2;
	int i, j, n;
	/* gives crude edges - pass through the junction filter */

	while (!complete) {
		complete = true;
		for (i = minr; i <= maxr; i++) {
			for (j = minc; j <= maxc; j++) {
				if (mEdges.at<uchar>(i, j)) {
					n = (mEdges.at<uchar>(i - 1, j - 1) != 0);
					n += 2 * (mEdges.at<uchar>(i, j - 1) != 0);
					n += 4 * (mEdges.at<uchar>(i + 1, j - 1) != 0);
					n += 8 * (mEdges.at<uchar>(i + 1, j) != 0);
					n += 16 * (mEdges.at<uchar>(i + 1, j + 1) != 0);
					n += 32 * (mEdges.at<uchar>(i, j + 1) != 0);
					n += 64 * (mEdges.at<uchar>(i - 1, j + 1) != 0);
					n += 128 * (mEdges.at<uchar>(i - 1, j) != 0);
					if (!jmap[n]) {
						mEdges.at<uchar>(i, j) = 0;
						complete = false;
					}
				}
			}
		}
	}
	return 0;
}

void DDMCMC_EdgePrcs::setParam(double dSigma) {
	this->dSigma = dSigma;
}

void DDMCMC_EdgePrcs::movePointIntoRegion(Point2i &p) {
	if (p.x < 0) p.x = 0;
	if (p.x >= cols) p.x = cols - 1;
	if (p.y < 0) p.y = 0;
	if (p.y >= rows) p.y = rows - 1;
}

int DDMCMC_EdgePrcs::getSign(int n) {
	if (n > 0) return 1;
	if (n == 0) return 0;
	return -1;
}
	

double DDMCMC_EdgePrcs::calcPercentThreshold(Mat mImage, float dPercent, int nBins) {
	if (mImage.channels() != 1) return -1;

	int nPixels = cols*rows;
	int channels[] = { 0 };
	int histSize[] = { nBins };
	double min, max;
	minMaxIdx(mImage, &min, &max);

	float range[] = { min, max };
	float sumN = 0, dMarker = nPixels*dPercent;
	const float* ranges[] = { range };
	Mat hist;

	calcHist(&mImage, 1, channels, Mat(), hist, 1, histSize, ranges);
	for (int i = 0; i < nBins; i++) {
		sumN += hist.at<float>(i);
		if (sumN >= dMarker) {
			return ((max-min) / nBins * i);
		}
	}
	return -1;
}

bool DDMCMC_EdgePrcs::isLocalMax(int ax, int ay) {
	double dMag = mEdgeStrength.at<float>(ay, ax);
	double x, y, absx, absy, maxc, minc, a, b, c, d, adjusted;

	if (ay <= 0 || ay >= rows - 1 || ax <= 0 || ax >= cols - 1)
		return false;
	if (dMag <= 0) 
		return false;

	x = Ix.at<float>(ay, ax);
	y = -Iy.at<float>(ay, ax);
	absx = fabs(x);
	absy = fabs(y);
	if (absx < absy) {
		maxc = absy;
		minc = absx;
		b = mEdgeStrength.at<float>(ay - 1, ax);
		c = mEdgeStrength.at<float>(ay + 1, ax);
		// *c*
		// ***
		// *b*
	}
	else {
		maxc = absx;
		minc = absy;
		b = mEdgeStrength.at<float>(ay, ax - 1);
		c = mEdgeStrength.at<float>(ay, ax + 1);
		// ***
		// b*c
		// ***
	}

	if (x * y > 0) { // same sign
		a = mEdgeStrength.at<float>(ay - 1, ax - 1);
		d = mEdgeStrength.at<float>(ay + 1, ax + 1);
		// **d
		// ***
		// a**
	}
	else {
		if (absx < absy) { // case 2 - steep
			a = mEdgeStrength.at<float>(ay - 1, ax + 1);
			d = mEdgeStrength.at<float>(ay + 1, ax - 1);
			// d**
			// ***
			// **a
		}
		else { // case 4 shallow
			a = mEdgeStrength.at<float>(ay + 1, ax - 1);
			d = mEdgeStrength.at<float>(ay - 1, ax + 1);
			// a**
			// ***
			// **d
		}
	}
	adjusted = (maxc + minc)*dMag;
	if (adjusted > maxc*b + minc*a && adjusted > maxc*c + minc*d)
		return true;
	return false;
}


int DDMCMC_EdgePrcs::getEdgeLocalMax() {
	float x, y, absx, absy, maxc, minc, a, b, c, d, adjusted;

	mLocalMaxCJ.setTo(Scalar::all(0));
	mLocalMax.setTo(Scalar::all(0));
	mLocalMaxFlag.setTo(Scalar::all(255));

	for (int y = 1; y < rows-1; y++)
		for (int x = 1; x < cols-1; x++) 
			if (isLocalMax(x, y)){
				mLocalMaxFlag.at<uchar>(y, x) = 0;
				mLocalMax.at<uchar>(y, x) = 255;
			}
	return 0;
}

void DDMCMC_EdgePrcs::setMatrixFlag(Mat m, PointList pointList, uchar flagValue) {
	PointNode *p = pointList.pHead;
	while (p != NULL) {
		if (p->data.x >= 0 && p->data.x < cols && p->data.y >= 0 && p->data.y < rows)
			m.at<uchar>(p->data.y, p->data.x) = flagValue;
		p = p->pNext;
	}
}

void DDMCMC_EdgePrcs::setContourMatrix(Contour *pContour, bool bFlag) {
	PointNode *current = pContour->pointList.pHead;
	Point2i point;
	while (current != NULL) {
		point = current->data;
		if (bFlag) mContourMatrix[point.y][point.x] = pContour;
		else mContourMatrix[point.y][point.x] = NULL;
		current = current->pNext;
	}
}

int DDMCMC_EdgePrcs::angle2Direction(double angle) {
	double deg = angle * 180;
	if (deg >= 157.5 || deg <= -157.5)
		return 5;
	else if (deg >= 112.5)
		return 4;
	else if (deg >= 57.5)
		return 3;
	else if (deg >= 22.5)
		return 2;
	else if (deg >= -22.5)
		return 1;
	else if (deg >= -77.5)
		return 8;
	else if (deg >= -112.5)
		return 7;
	else
		return 6;
}

Point2i DDMCMC_EdgePrcs::direction2Point(int nDir) {
	Point2i p;
	switch (nDir)
	{
	case 0:
		p.x = 0; p.y = 0;
		break;
	case 1:
		p.x = 1; p.y = 0;
		break;
	case 2:
		p.x = 1; p.y = 1;
		break;
	case 3:
		p.x = 0; p.y = 1;
		break;
	case 4:
		p.x = -1; p.y = 1;
		break;
	case 5:
		p.x = -1; p.y = 0;
		break;
	case 6:
		p.x = -1; p.y = -1;
		break;
	case 7:
		p.x = 0; p.y = -1;
		break;
	case 8:
		p.x = 1; p.y = -1;
		break;
	default:
		p.x = 0; p.y = 0;
		break;
	}
	return p;
}

Vec2d DDMCMC_EdgePrcs::direction2Vector(int nDir) {
	Vec2d v;
	switch (nDir) {
	case 0:
		v[0] = 0; v[1] = 0;
		break;
	case 1:
		v[0] = 1; v[1] = 0;
		break;
	case 2:
		v[0] = 1; v[1] = 1;
		break;
	case 3:
		v[0] = 0; v[1] = 1;
		break;
	case 4:
		v[0] = -1; v[1] = 1;
		break;
	case 5:
		v[0] = -1; v[1] = 0;
		break;
	case 6:
		v[0] = -1; v[1] = -1;
		break;
	case 7:
		v[0] = 0; v[1] = -1;
		break;
	case 8:
		v[0] = 1; v[1] = -1;
		break;
	default:
		v[0] = 0; v[1] = 0;
		break;
	}
	return v;
}

double DDMCMC_EdgePrcs::direction2Angle(double dAngle, int nDir) {
	assert(dAngle >= 0);
	switch (nDir) {
	case 2: case 3: case 4:
		return dAngle;
		break;
	case 6: case 7: case 8:
		return dAngle - PI;
		break;
	case 1:
		if (dAngle <= PI/2) return dAngle;
		else return dAngle - PI;
		break;
	case 5:
		if (dAngle >= PI/2) return dAngle;
		else return dAngle - PI;
		break;
	}
	return 0;
}

double DDMCMC_EdgePrcs::calcAngle(Vec2d v) {
	return atan2(v[1], v[0]);
}

double DDMCMC_EdgePrcs::calcLength(Vec2d v) {
	return sqrt(v[0] * v[0] + v[1] * v[1]);
}

double DDMCMC_EdgePrcs::calcCompatibility(Vec2d v, Vec2d v1, Vec2d v2) {
	return sin(fabs(calcAngle(v1) - calcAngle(v2))) + SPL*calcLength(v);
}

void DDMCMC_EdgePrcs::cutContour(Contour *pContour, Point2i p1, int endNo) {
	Contour *pCutContour = new Contour();
	Point2i p;

	if (endNo == 0) {
//		keep the head for pContour, cut the tail for pCutContour, search for tail
		do {
			p = pContour->pointList.removeTail();
			pCutContour->pointList.addHead(p);
			mContourMatrix[p.y][p.x] = pCutContour;
		} while (p != p1); 

		pCutContour->bExtendHeadTail[0] = false;
		pCutContour->bExtendHeadTail[1] = pContour->bExtendHeadTail[1];
		pCutContour->nHeadTailStatus[0] = 1; // reach other contour
		pCutContour->nHeadTailStatus[1] = pContour->nHeadTailStatus[1];
		memcpy(pCutContour->aTailNeighbor, pContour->aTailNeighbor, sizeof(ContourNeighbor)*pContour->nTailNeighbors);
		pContour->nTailNeighbors = 0;
	}
	else {
//		keep the tail for pContour, cut the head for pCutContour, search for head
		do {
			p = pContour->pointList.removeHead();
			pCutContour->pointList.addTail(p);
			mContourMatrix[p.y][p.x] = pCutContour;
		} while (p != p1);

		pCutContour->bExtendHeadTail[0] = pContour->bExtendHeadTail[0];
		pCutContour->bExtendHeadTail[1] = false;
		pCutContour->nHeadTailStatus[1] = 1; // reach other contour
		pCutContour->nHeadTailStatus[0] = pContour->nHeadTailStatus[0];
		memcpy(pCutContour->aHeadNeighbor, pContour->aHeadNeighbor, sizeof(ContourNeighbor)*pContour->nHeadNeighbors);
		pContour->nHeadNeighbors = 0;
	}
//	add the cut contour to the whole image
	contourList.addTail(pCutContour);
}

void DDMCMC_EdgePrcs::combineContour(Contour *pContour1, int endNo1, 
	Contour* pContour2, int endNo2) {
	Point2i p;
	PointNode *pCurrent;
	if (endNo1 == 0) { // head
		if (endNo2 == 0) {
			//combine pC2'head to pC1's head
			pCurrent = pContour2->pointList.pHead;
			while (pCurrent != NULL) {
				p = pCurrent->data;
				pContour1->pointList.addHead(p);
				mContourMatrix[p.y][p.x] = pContour1;
				pCurrent = pCurrent->pNext;
			}
			memcpy(pContour1->aHeadNeighbor, pContour2->aTailNeighbor, 
				sizeof(ContourNeighbor)*pContour2->nTailNeighbors);
			pContour1->nHeadNeighbors = pContour2->nTailNeighbors;
			pContour1->bExtendHeadTail[0] = pContour2->bExtendHeadTail[1];
		} else {
			// combine PC2's tail to PC1's head
			pCurrent = pContour2->pointList.pTail;
			while (pCurrent != NULL) {
				p = pCurrent->data;
				pContour1->pointList.addHead(p);
				mContourMatrix[p.y][p.x] = pContour1;
				pCurrent = pCurrent->pPrev;
			}
			memcpy(pContour1->aHeadNeighbor, pContour2->aHeadNeighbor,
				sizeof(ContourNeighbor)*pContour2->nHeadNeighbors);
			pContour1->nHeadNeighbors = pContour2->nHeadNeighbors;
			pContour1->bExtendHeadTail[0] = pContour2->bExtendHeadTail[0];
		}
	} else { // Tail
		if (endNo2 == 0) {
			// combine PC2's head to PC1's tail
			pCurrent = pContour2->pointList.pHead;
			while (pCurrent != NULL) {
				p = pCurrent->data;
				pContour1->pointList.addTail(p);
				mContourMatrix[p.y][p.x] = pContour1;
				pCurrent = pCurrent->pNext;
			}
			memcpy(pContour1->aTailNeighbor, pContour2->aTailNeighbor,
				sizeof(ContourNeighbor)*pContour2->nTailNeighbors);
			pContour1->nTailNeighbors = pContour2->nTailNeighbors;
			pContour1->bExtendHeadTail[1] = pContour2->bExtendHeadTail[1];
		} else {
			//combine pC2's tail to pC1's tail
			pCurrent = pContour2->pointList.pTail;
			while (pCurrent != NULL) {
				p = pCurrent->data;
				pContour1->pointList.addTail(p);
				mContourMatrix[p.y][p.x] = pContour1;
				pCurrent = pCurrent->pPrev;
			}
			memcpy(pContour1->aTailNeighbor, pContour2->aHeadNeighbor,
				sizeof(ContourNeighbor)*pContour2->nHeadNeighbors);
			pContour1->nTailNeighbors = pContour2->nHeadNeighbors;
			pContour1->bExtendHeadTail[1] = pContour2->bExtendHeadTail[0];
		}
	}
	contourList.remove(pContour2);
}

int DDMCMC_EdgePrcs::traceContour4OverSegment(int endNo, Contour*pContour) {
	Point2i p0, p1, dp; //p0: point where the search is from; p1: candidate extension point
	Vec2d v, v1, v2;
	int dir, bestDir = -1, searchDir, nReturn = 0;
	double dBestCompat = HUGE_PENALTY;
	double dCompat = dBestCompat;

	if (pContour->nLastMoveDir == 0) {
		//		no last move direction, get the direction from the tail point angle
		if (endNo == 1)
			pContour->nLastMoveDir = angle2Direction(pContour->dAngleTail);
		else // extend head
			pContour->nLastMoveDir = angle2Direction(pContour->dAngleHead);
	}

	if (endNo == 1)
		p0 = pContour->pointList.pTail->data;
	else 
		p0 = pContour->pointList.pHead->data;
	
	dir = pContour->nLastMoveDir;
	p1.x = p0.x;
	p1.y = p0.y;
	if (p0.x < 1 || p0.x > cols - 2 || p0.y < 1 || p0.y > rows - 2) {
//		reach the boundary, mark it
		pContour->nHeadTailStatus[endNo] = 2;
		pContour->bExtendHeadTail[endNo] = false;
		return 1;
	}

	for (int i = 0; i < 9; i++) {
		searchDir = mSearchDirection[dir][i];
		if (searchDir == 0) break;
		dp = direction2Point(searchDir);
		p1 = p0 + dp;
		if (p1.x<1 || p1.x > cols - 2 || p1.y<1 || p1.y>rows - 2) {
//			reach the boundary, mark it
			pContour->nHeadTailStatus[endNo] = 2;
			bestDir = searchDir;
			pContour->bExtendHeadTail[endNo] = false;
			nReturn = 1;
			break;
		}

		if (mLocalMaxFlag.at<uchar>(p1.y, p1.x) != 0) continue;

		if (mContourMatrix[p1.y][p1.x] != NULL) {
			// meet other contour, stop, divide the met contour
			Contour *pMetContour = mContourMatrix[p1.y][p1.x];
			if (pMetContour == pContour) {
				// meet itself
				int nCloseLength = pContour->getLengthFromPoint2End(endNo, p1);
				if (nCloseLength >= 10 * dSigma) {
					// large close area					
					if  (p1 != pContour->pointList.getHeadOrTail(endNo)){
						//	meet the inside of itself, cut it and keep the cut part to a new contour
						cutContour(pContour, p1, endNo);
					}
					// else need no cut
					pContour->nHeadTailStatus[endNo] = 0;
					pContour->nHeadTailStatus[1-endNo] = 0;
					bestDir = searchDir;
					return 1;
				} else continue;
			} else {
				// Meet other contour
				if (p1 == pMetContour->pointList.pHead->data) {
					// meet the head of that contour
					combineContour(pContour, endNo, pMetContour, 0);
					return 1;
				}
				else if (p1 == pMetContour->pointList.pTail->data) {
					// meet the tail of that contour
					combineContour(pContour, endNo, pMetContour, 1);
					return 1;
				} else {
					// meet inside of that contour
					cutContour(pMetContour, p1, endNo);
					pContour->nHeadTailStatus[endNo] = 1;
					bestDir = searchDir;
					return 1;
				} 
			} // else meet other contour
		} 
		v = direction2Vector(searchDir);
		v1[0] = Iy.at<float>(p0.y, p0.x);
		v1[1] = Ix.at<float>(p0.y, p0.x);
		v2[0] = Iy.at<float>(p1.y, p1.x);
		v2[1] = Ix.at<float>(p1.y, p1.x);
		dCompat = calcCompatibility(v, v1, v2);
		if (bestDir == -1 || dBestCompat > dCompat) {
			// only search in the same level
			bestDir = searchDir;
			dBestCompat = dCompat;
		}
	}

	// nFlag - 0: trace only if finding the point at the same level
	if (bestDir == -1) {
		// not found the next search point in the same level, stop tracing
		return 1;
	}	// else, find tracing point at the same level, the direction is bestDir

	pContour->nLastMoveDir = bestDir;
	dp = direction2Point(bestDir);
	p1 = p0 + dp;

	// new valid point, add it to the contour
	if (endNo == 1) {
		pContour->pointList.addTail(p1);
		pContour->dAngleTail = direction2Angle(mEdgeAngle.at<float>(p1.y, p1.x), bestDir);
	} else {
		pContour->pointList.addHead(p1);
		pContour->dAngleHead = direction2Angle(mEdgeAngle.at<float>(p1.y, p1.x), bestDir);
	}
	mContourMatrix[p1.y][p1.x] = pContour;
	return nReturn;
}

void DDMCMC_EdgePrcs::getContourAtPoint4OverSegment(Point2i p, Contour *pNewContour) {
	int x = p.x, y = p.y;
	pNewContour->pointList.addHead(p);
	mContourMatrix[y][x] = pNewContour;
	pNewContour->dAngleHead = mEdgeAngle.at<float>(y, x);
	pNewContour->dAngleTail = pNewContour->dAngleHead - PI;
	if (pNewContour->dAngleTail == 0)
		pNewContour->dAngleTail = -PI;
	for (int i = 0; i < 2; i++) {
		int nFlag = 0;
		pNewContour->nLastMoveDir = 0;
		while (nFlag == 0 && pNewContour->pointList.nSize <= nMaxContourLength
			&& pNewContour->bExtendHeadTail[i])
			nFlag = traceContour4OverSegment(i, pNewContour);
	}
}

// 
void DDMCMC_EdgePrcs::buildContourList4OverSegment() {
	Point2i p;
	Contour *pNewContour = new Contour();

	mContourMatrix = new Contour**[rows];
	for (int y = 0; y < rows; y++) {
		mContourMatrix[y] = new Contour*[cols];
		for (int x = 0; x < cols; x++)
			mContourMatrix[y][x] = NULL;
	}

	getEdgeLocalMax();

//	Get the contours
//	set the boader as a contour
	for (int x = 0; x < cols; x++) {
		p.x = x;
		p.y = 0;
		pNewContour->pointList.addTail(p);
		mLocalMaxFlag.at<uchar>(p.y, p.x) = 0;
		mContourMatrix[p.y][p.x] = pNewContour;
		mEdgeAngle.at<float>(p.y, p.x) = 0;
	}

	for (int y = 1; y < rows; y++) {
		p.x = cols - 1;
		p.y = y;
		pNewContour->pointList.addTail(p);
		pNewContour->pointList.addTail(p);
		mLocalMaxFlag.at<uchar>(p.y, p.x) = 0;
		mContourMatrix[p.y][p.x] = pNewContour;
		mEdgeAngle.at<float>(p.y, p.x) = PI / 2;
	}

	for (int x = cols - 2; x >= 0; x--) {
		p.x = x;
		p.y = rows - 1;
		pNewContour->pointList.addTail(p);
		pNewContour->pointList.addTail(p);
		mLocalMaxFlag.at<uchar>(p.y, p.x) = 0;
		mContourMatrix[p.y][p.x] = pNewContour;
		mEdgeAngle.at<float>(p.y, p.x) = PI;
	}

	for (int y = rows - 2; y > 0; y--) {
		p.x = 0;
		p.y = y;
		pNewContour->pointList.addTail(p);
		pNewContour->pointList.addTail(p);
		mLocalMaxFlag.at<uchar>(p.y, p.x) = 0;
		mContourMatrix[p.y][p.x] = pNewContour;
		mEdgeAngle.at<float>(p.y, p.x) = PI / 2;
	}

	pNewContour->bExtendHeadTail[0] = false;
	pNewContour->bExtendHeadTail[1] = false;
	contourList.addTail(pNewContour);
	setMatrixFlag(mLocalMaxCJ, pNewContour->pointList, 255);
	for (int y = 0; y < rows; y++) {
		for (int x = 0; x < cols; x++) // Search every point in the image.
			if (mContourMatrix[y][x] == NULL && mLocalMaxFlag.at<uchar>(y, x) == 0) {
				// If it is local max and does not belong to other contours,
				// then find a new contour.
				pNewContour = new Contour();
				p.x = x;
				p.y = y;
				getContourAtPoint4OverSegment(p, pNewContour);
				if (nMinContourLength > 1) {
					if (pNewContour->pointList.nSize >= nMinContourLength) {
						contourList.addTail(pNewContour);
						setMatrixFlag(mLocalMaxCJ, pNewContour->pointList, 255);
					} else {
						setContourMatrix(pNewContour, false);
						delete pNewContour;
					}
				} else {
					contourList.addTail(pNewContour);
					setMatrixFlag(mLocalMaxCJ, pNewContour->pointList, 255);
				}
				//imshow("test", mLocalMaxCJ);
				//waitKey();
		}
	}
}

void DDMCMC_EdgePrcs::findNeighborMovingPts(Point2i p, int &n, Point2i &p1, 
	Point2i &p2, Point2i &p3) {
	p1 = p;
	if (abs(p.x) != abs(p.y)) {
		n = 1;
		if (abs(p.x) > abs(p.y)) p1.x += getSign(p1.x);
		else p1.y += getSign(p1.y);
	} else {
		n = 3;
		p2 = p;
		p3 = p;
		p1.x += getSign(p1.x);
		p2.x += getSign(p2.x);
		p2.y += getSign(p2.y);
		p3.y += getSign(p3.y);
	}
}

bool DDMCMC_EdgePrcs::isNeighbor(Point2i p1, Point2i p2) {
	if (abs(p1.x - p2.x) <= 1 && abs(p1.y - p2.y) <= 1) return true;
	return false;
}

double DDMCMC_EdgePrcs::calcContinuityStrength(Contour *pContour1, int &endNo1, Contour *pContour2, int &endNo2, Point2i p) {
	double aFactor[10], dSumFactor = 0, dAngle1, aAngle2[3];
	double dEdgeStrength1 = 1, dEdgeStrength2 = 1; // Questions here, check the result
	double aCntStrength[3];
	Point2i p1, aP2[3];
	int nLength1 = pContour1->pointList.nSize;
	int nLength2 = pContour2->pointList.nSize;
	int nPoints4Compare = 3;

	aFactor[0] = 1;
	aFactor[1] = 0;
	aFactor[2] = 0;

	for (int i = 0; i < 3; i++)
		dSumFactor += aFactor[i];

	if (endNo1 == 0) {
		p1 = pContour1->pointList.pHead->data;
		dAngle1 = pContour1->dAngleHead;
	}
	else {
		p1 = pContour1->pointList.pTail->data;
		dAngle1 = pContour1->dAngleTail;
	}

	aP2[0] = pContour2->pointList.pHead->data;
	aP2[1] = pContour2->pointList.pTail->data;
	aP2[2] = p;

	if (aP2[2] == aP2[0] || aP2[2] == aP2[1])
		nPoints4Compare = 2;
	aAngle2[0] = pContour2->dAngleHead;
	aAngle2[1] = pContour2->dAngleTail;
	aAngle2[2] = mEdgeAngle.at<float>(p.y, p.x);

	for (int i = 0; i < nPoints4Compare; i++) {
		double dis = abs(p1.x - aP2[i].x) + abs(p1.y - aP2[i].y);
		if (dis != 0) dis = 1.0 / dis;

		double dAngle = 1 / (fabs(fabs(dAngle1 - aAngle2[i]) - PI)*180 + 1);
		double dEdgeStr = (dEdgeStrength1 + dEdgeStrength2) / 2;
		aCntStrength[i] = (dis*aFactor[0] + dAngle*aFactor[1] + dEdgeStr*aFactor[2]) / dSumFactor;
	}

	if (pContour1 == contourList.pHead || pContour2 == contourList.pHead)
		aCntStrength[2] /= 2;
	else 
		aCntStrength[2] = aCntStrength[2]*2/3;

	endNo2 = 0;
	for (int i = 1; i < nPoints4Compare; i++) 
		if (aCntStrength[i] >= aCntStrength[endNo2]) 
			endNo2 = i;
	
	return aCntStrength[endNo2];
}

void DDMCMC_EdgePrcs::getNeighborOfContour(Contour *pContour, int endNo) {
	// get the contour map to find the neighbors of each contour
	Point2i *pPtArray1 = new Point2i[(rows + cols) * 2];
	Point2i *pPtArray2 = new Point2i[(rows + cols) * 2];
	Point2i p, pt, dp, pt_n, p1, p2, p3;
	ContourNeighbor *pNbrArray, cntNeighbor;
	Contour *pCheckingContour;
	int nPts1, nPts2, nNeighbors, nContourLength, nSearchRadius, nSearchLimit, endNo_n;
	double dStrength;
	bool bContinue = false, bBreak = false;

	p = pContour->pointList.getHeadOrTail(endNo);
	pt = p;

	// pPtArray1 is for points being checked, it is initiated to 8 connection points
	// pPtArray2 is for futher checked points
	memcpy(pPtArray1, aPointNeighbor, 8 * sizeof(Point2i)); 
	nPts1 = 8;
	nPts2 = 0;

	if (endNo == 0) pNbrArray = pContour->aHeadNeighbor;
	else pNbrArray = pContour->aTailNeighbor;
	nContourLength = pContour->pointList.nSize;
	nSearchRadius = 0;
	nSearchLimit = int(nContourLength + dSigma);

	while ((nPts1 > 0 || nPts2 > 0) && pContour->getNeighborNum(endNo) < MAX_NBR_NUM) {
		if (pContour->bExtendHeadTail[1 - endNo] && nSearchRadius > nSearchLimit)
			break; // the other end and the serach area is too large
		if (nPts1 <= 0) {
			memcpy(pPtArray1, pPtArray2, sizeof(Point2i)*nPts2);
			nPts1 = nPts2;
			nPts2 = 0;
			nSearchRadius++;
		}
		dp = pPtArray1[nPts1 - 1];
		nPts1--;
		pt_n = pt + dp;
		if (pt_n.x <0 || pt_n.x > cols - 1 || pt_n.y <0 || pt_n.y > rows - 1)
			continue;
		else { // not reaching boundary
			pCheckingContour = mContourMatrix[pt_n.y][pt_n.x];
			if (pCheckingContour == NULL) {
				// if no contour at pt_n, add 1 or 3 points to pPtArray2
				int n = 0;
				findNeighborMovingPts(dp, n, p1, p2, p3);
				if (n == 1) {
					pPtArray2[nPts2] = p1;
					nPts2++;
				}
				else if (n == 3) {
					pPtArray2[nPts2] = p1;
					nPts2++;
					pPtArray2[nPts2] = p2;
					nPts2++;
					pPtArray2[nPts2] = p3;
					nPts2++;
				}
				continue; // next point
			}

			// there is a contour at pt_n
			if (pCheckingContour == pContour) { // the same as pContour, next point
				if (pt_n == pContour->pointList.getHeadOrTail(1 - endNo)) {
					int nLength = pContour->getLengthFromPoint2End(endNo, pt_n);
					if (nLength <= 20 * dSigma || nLength <= 2) continue;
				}
				else continue;
			}

			// Check if current checking contour is already in the neighbor list
			bContinue = false;
			nNeighbors = pContour->getNeighborNum(endNo);
			for (int j = 0; j < nNeighbors; j++)
				if (pCheckingContour == pNbrArray[j].pContour) {
					// Already in the neighbor list, next point
					bContinue = true;
					break;
				}
			if (bContinue) continue;
		} // else not reach boundary
		
		// add the checking point to the neighbor
		cntNeighbor.pContour = pCheckingContour;
		endNo_n = 0;
		dStrength = calcContinuityStrength(pContour, endNo, pCheckingContour, endNo_n, pt_n);
		cntNeighbor.dContinuityStrength = dStrength;
		cntNeighbor.nEnd = endNo_n;
		cntNeighbor.point = pt_n;
		pContour->addNeighbor(endNo, cntNeighbor);
	}
	delete[] pPtArray1;
	delete[] pPtArray2;
}

bool DDMCMC_EdgePrcs::contourCrossSegLine(Contour &linkContour) {
	Point2i p, p1, p2;
	PointNode *pNode = linkContour.pointList.pHead;
	while (pNode != NULL) {
		p = pNode->data;
		if (mLocalMaxCJ.at<uchar>(p.y, p.x) == 255)
			return true;

		// Deal with diagonal cross
		p1.x = p.x - 1;
		p1.y = p.y;
		p2.x = p.x;
		p2.y = p.y - 1;
		movePointIntoRegion(p1);
		movePointIntoRegion(p2); 
		if (mLocalMaxCJ.at<uchar>(p1.y, p1.x) == 255
			&& mLocalMaxCJ.at<uchar>(p2.y, p2.x) == 255)
			return true;
		p2.y = p.y + 1;
		movePointIntoRegion(p2);
		if (mLocalMaxCJ.at<uchar>(p1.y, p1.x) == 255
			&& mLocalMaxCJ.at<uchar>(p2.y, p2.x) == 255)
			return true;
		p1.x = p.x + 1;
		movePointIntoRegion(p1);
		if (mLocalMaxCJ.at<uchar>(p1.y, p1.x) == 255
			&& mLocalMaxCJ.at<uchar>(p2.y, p2.x) == 255)
			return true;
		p2.y = p.y - 1;
		movePointIntoRegion(p2);
		if (mLocalMaxCJ.at<uchar>(p1.y, p1.x) == 255
			&& mLocalMaxCJ.at<uchar>(p2.y, p2.x) == 255)
			return true;
		pNode = pNode->pNext;
	}
	return false;
}

void DDMCMC_EdgePrcs::freeOneEndOfContour(Contour *pContour, Contour *pContourLinked, int endNo) {
	ContourNeighbor contourNbr;
	Contour *pNbrContour;
	Point2i p1; // the end of pContour
	Point2i p2; // the end of pNbrContour
	double dExtendNbrStrength;
	int nEndNbrContour, nNeighbors;
	bool bExtend = false;

	while (pContour->bExtendHeadTail[endNo]) {
		// Still need extension
		getNeighborOfContour(pContour, endNo);
		pContour->sortNeighbors(endNo);
		nNeighbors = pContour->getNeighborNum(endNo);
		if (nNeighbors <= 0) { // no neighbors
			setMatrixFlag(mLocalMaxCJ, pContour->pointList, 0);
			break;
		}

		for (int j = 0; j < nNeighbors; j++) {
			contourNbr = pContour->getNeighbor(endNo, j);
			nEndNbrContour = contourNbr.nEnd;
			pNbrContour = contourNbr.pContour;
			if (pNbrContour == pContourLinked) {
				if (!bExtend) {
					bExtend = true;
					dExtendNbrStrength = contourNbr.dContinuityStrength;
					pContour->bExtendHeadTail[endNo] = false;
				}
				continue;
			}

			if (bExtend) { // extend other neighbors
				if (contourNbr.dContinuityStrength <= dExtendNbrStrength * 3 / 4)
					break;
				if (nEndNbrContour == 2)
					continue; // the link point on the neighbor is inside point, not end
				p1 = pContour->pointList.getHeadOrTail(endNo);
				p2 = pContour->pointList.getHeadOrTail(nEndNbrContour);
				if (isNeighbor(p1, p2)) continue;

				Contour linkContour(p1, p2); // nLevel not set
				if (contourCrossSegLine(linkContour)) // link contour corss other contour
					continue;
				else  // it is allowed to link
					setMatrixFlag(mLocalMaxCJ, linkContour.pointList, 255);
				continue;
			}

			if (pNbrContour == pContour) { // next neighbor is the original contour
				p1 = pContour->pointList.getHeadOrTail(endNo);
				if (nEndNbrContour != 2) // end point
					p2 = pNbrContour->pointList.getHeadOrTail(nEndNbrContour);
				else /// inside point
					p2 = contourNbr.point;

				if (!isNeighbor(p1, p2)) { // p1 and p2 are not neighbor
					Contour linkContour(p1, p2);
					if (contourCrossSegLine(linkContour)) // link contour corss other contour
						continue;
					else // it is allowed to link
						setMatrixFlag(mLocalMaxCJ, linkContour.pointList, 255);
				}

				if (endNo + nEndNbrContour == 1) { // the line is closed
					pContour->bExtendHeadTail[0] = false;
					pContour->bExtendHeadTail[1] = false;
				}
				else if (nEndNbrContour == 2)  // reach the inside point of contour
					pContour->bExtendHeadTail[endNo] = false;
				else // reach the same end of itself
					pContour->bExtendHeadTail[endNo] = false;
				bExtend = true;
				dExtendNbrStrength = contourNbr.dContinuityStrength;
				continue;
			} // else, the neighbor is not itself

			p1 = pContour->pointList.getHeadOrTail(endNo);
			if (nEndNbrContour != 2)
				p2 = pNbrContour->pointList.getHeadOrTail(nEndNbrContour);
			else
				p2 = contourNbr.point;

			if (!isNeighbor(p1, p2)) {
				Contour linkContour(p1, p2);
				if (contourCrossSegLine(linkContour)){ // link contour cross other contour
					if (nEndNbrContour == 2) continue;
					p2 = contourNbr.point;
					Contour linkContour2(p1, p2);
					if (contourCrossSegLine(linkContour2)) continue;
					linkContour = linkContour2;
				}

				setMatrixFlag(mLocalMaxCJ, linkContour.pointList, 255);
				pContour->bExtendHeadTail[endNo] = false;
				if (nEndNbrContour != 2)
					freeOneEndOfContour(pNbrContour, pContour, nEndNbrContour);
				bExtend = true;
				dExtendNbrStrength = contourNbr.dContinuityStrength;
				continue; // next neighbor contour
			}
			else { // p1 and p2 are neighbor
				if (nEndNbrContour != 2)
					freeOneEndOfContour(pNbrContour, pContour, nEndNbrContour);
				pContour->bExtendHeadTail[endNo] = false;
				bExtend = true;
				dExtendNbrStrength = contourNbr.dContinuityStrength;
				continue; // next neighbor contour
			}
		}

		if (!bExtend) {
			pContour->bExtendHeadTail[endNo] = false;
		}
	}
}

void DDMCMC_EdgePrcs::freeEndsOfAllContours() {
	Contour * pContour = contourList.pHead;
	while (pContour != NULL) {
		// for each contour in this level, de-free its' ends
		// extend this contour
		// what is considered is in the whole image, not in the specialized region
		freeOneEndOfContour(pContour, NULL, 0);
		freeOneEndOfContour(pContour, NULL, 1);
		pContour = pContour->pNext;
	}
}

void DDMCMC_EdgePrcs::labelRegionFromSeed(Mat &mRegionLabel, int x, int y, Point2i *pPoints) {
	Point2i aNbrPoints[4];
	int x1, y1, x2, y2, n = 0;

	aNbrPoints[0].x = 1;
	aNbrPoints[0].y = 0;
	aNbrPoints[1].x = 0;
	aNbrPoints[1].y = 1;
	aNbrPoints[2].x = -1;
	aNbrPoints[2].y = 0;
	aNbrPoints[3].x = 0;
	aNbrPoints[3].y = -1;

	pPoints[0].x = x;
	pPoints[0].y = y;
	mRegionLabel.at<int>(pPoints[n].y, pPoints[n].x) = nLabels;

	while (n >= 0) {
		x1 = pPoints[n].x;
		y1 = pPoints[n].y;
		for (int i = 0; i < 4; i++) {
			x2 = x1 + aNbrPoints[i].x;
			y2 = y1 + aNbrPoints[i].y;
			if (x2 >= 0 && x2 < cols && y2 >= 0 && y2 < rows
				&& mRegionLabel.at<int>(y2, x2) == -1 && mLocalMaxCJ.at<uchar>(y2, x2) != 255) {
				mRegionLabel.at<int>(y2, x2) = nLabels;
				pPoints[n].y = y2;
				pPoints[n].x = x2;
				n++;
			}
		}
		n--;
	}
}

void DDMCMC_EdgePrcs::labelBoundaries(Mat &mRegionLabel) {
	Mat mTempMCMC = mRegionLabel;
	int x1, y1, nMaxDim = max(cols, rows);
	bool bContinue = true;

	while (bContinue) {
		bContinue = false;
		for (int y = 0; y < rows; y++)
			for (int x = 0; x < cols; x++) {
				if (mTempMCMC.at<int>(y, x) < 0) {
					x1 = x + 1;
					y1 = y;		// left neighbor
					if (x1 >= 0 && x1 < cols && y1 >= 0 && y1 <rows 
						&& mTempMCMC.at<int>(y1, x1) >= 0) {
						mRegionLabel.at<int>(y, x) = mTempMCMC.at<int>(y1, x1);
						continue;
					}

					x1 = x;
					y1 = y + 1; // above neighbor
					if (x1 >=0 && x1 < cols && y1 >= 0 && y1 < rows
						&& mTempMCMC.at<int>(y1, x1) >= 0) {
						mRegionLabel.at<int>(y, x) = mTempMCMC.at<int>(y1, x1);
						continue;
					}

					x1 = x - 1;
					y1 = y;		// right neighbor
					if (x1 >= 0 && x1 < cols && y1 >= 0 && y1 < rows
						&& mTempMCMC.at<int>(y1, x1) >= 0) {
						mRegionLabel.at<int>(y, x) = mTempMCMC.at<int>(y1, x1);
						continue;
					}

					x1 = x;
					y1 = y - 1;
					if (x1 >= 0 && x1 < cols && y1 >= 0 && y1 < rows
						&& mTempMCMC.at<int>(y1, x1) >= 0) {
						mRegionLabel.at<int>(y, x) = mTempMCMC.at<int>(y1, x1);
						continue;
					}
					bContinue = true;
				}
			}
		mTempMCMC = mRegionLabel;
	}
}

void DDMCMC_EdgePrcs::labelRegions(Mat &mRegionLabel) {
	Point2i *pPoints = new Point2i[cols*rows];
	mRegionLabel.setTo(Scalar::all(-1));
	nLabels = 0;

	for (int y = 0; y < rows; y++) 
		for (int x = 0; x < cols; x++) {
			if (mLocalMaxCJ.at<uchar>(y, x) != 255 && mRegionLabel.at<int>(y, x) == -1) {
				labelRegionFromSeed(mRegionLabel, x, y, pPoints);
				nLabels++;
			}
		}
	
	delete[] pPoints;
	labelBoundaries(mRegionLabel);
}

void DDMCMC_EdgePrcs::detectEdges() {
	Mat mGray, mBlurred, Ixy;
	float threshHigh, threshLow, median, alpha = 0.33;

	cvtColor(mRawImage, mGray, COLOR_BGR2GRAY);
	GaussianBlur(mGray, mBlurred, Size(0, 0), dSigma);

	//	Calculate the gradient along two directions
	Ixy.create(rows, cols, CV_32F);
	Sobel(mBlurred, Ix, CV_32F, 1, 0, 3);
	Sobel(mBlurred, Iy, CV_32F, 0, 1, 3);
	for (int y = 0; y < rows; y++)
		for (int x = 0; x < cols; x++)
			Ixy.at<float>(y, x) = float(sqrt(pow(Ix.at<float>(y, x), 2) + pow(Iy.at<float>(y, x), 2)));

	//	Canny edge detection
	median = calcPercentThreshold(mBlurred, 0.5, 50);
	threshLow = 0.33 * median;
	threshHigh = 2 * 0.33 * median;

	if (bColor) {
		Canny(mBlurred, mEdges, threshLow, threshHigh);
	}
	skeletonize(mEdges);
	//imshow("new", mEdges);
	//waitKey();

	//	Calculate angles and strengthes
	mEdgeStrength.setTo(Scalar::all(0));
	for (int y = 0; y < rows; y++)
		for (int x = 0; x < cols; x++) {
			if (mEdges.at<uchar>(y, x) >0)
				mEdgeStrength.at<float>(y, x) = max(Ixy.at<float>(y, x), 15.0);
			float angle = atan2(Iy.at<float>(y, x), Ix.at<float>(y, x));
			if (angle < 0) angle += PI;
			mEdgeAngle.at<float>(y, x) = angle;
		}
}

// Segment and label regions
// Only one level
void DDMCMC_EdgePrcs::segmentRegions(double dSigma, Mat &mRegionLabel) {
	setParam(dSigma);
	mRegionLabel.create(rows, cols, CV_32S);

	detectEdges();
	buildContourList4OverSegment();
	//printMatrix(mLocalMaxCJ, "localMatrix0_before.txt");
	imshow("localMax", mLocalMaxCJ);
	waitKey();
	freeEndsOfAllContours();
	//printMatrix(mLocalMaxCJ, "localMatrix0_after.txt");
	imshow("localMax", mLocalMaxCJ);
	waitKey();
	labelRegions(mRegionLabel);
}

